/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include "image_transport_decompressor/nodelet.h"

namespace image_preprocessor
{
void ImageTransportDecompressorNodelet::onInit()
{
  nh_ = getNodeHandle();
  pnh_ = getPrivateNodeHandle();
  pnh_.param("encoding", enc_, std::string("default"));

  compressed_image_sub_ = pnh_.subscribe(
    "input/compressed_image", 1, &ImageTransportDecompressorNodelet::compressedImageCallback, this);
  raw_image_pub_ = pnh_.advertise<sensor_msgs::Image>("output/raw_image", 1);
}

void ImageTransportDecompressorNodelet::compressedImageCallback(
  const sensor_msgs::CompressedImageConstPtr & input_compressed_image_msg)

{
  cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
  // Copy message header
  cv_ptr->header = input_compressed_image_msg->header;

  // Decode color/mono image
  try {
    cv_ptr->image = cv::imdecode(cv::Mat(input_compressed_image_msg->data), cv::IMREAD_COLOR);

    // Assign image encoding string
    const size_t split_pos = input_compressed_image_msg->format.find(';');
    if (split_pos == std::string::npos) {
      // Older version of compressed_image_transport does not signal image format
      switch (cv_ptr->image.channels()) {
        case 1:
          cv_ptr->encoding = sensor_msgs::image_encodings::MONO8;
          break;
        case 3:
          cv_ptr->encoding = sensor_msgs::image_encodings::BGR8;
          break;
        default:
          ROS_ERROR("Unsupported number of channels: %i", cv_ptr->image.channels());
          break;
      }
    } else {
      std::string image_encoding;
      if (enc_ == std::string("default"))
        image_encoding = input_compressed_image_msg->format.substr(0, split_pos);
      else if (enc_ == std::string("rgb8"))
        image_encoding = "rgb8";
      else if (enc_ == std::string("bgr8"))
        image_encoding = "bgr8";
      else
        image_encoding = input_compressed_image_msg->format.substr(0, split_pos);

      cv_ptr->encoding = image_encoding;

      if (sensor_msgs::image_encodings::isColor(image_encoding)) {
        std::string compressed_encoding = input_compressed_image_msg->format.substr(split_pos);
        bool compressed_bgr_image =
          (compressed_encoding.find("compressed bgr") != std::string::npos);

        // Revert color transformation
        if (compressed_bgr_image) {
          // if necessary convert colors from bgr to rgb
          if (
            (image_encoding == sensor_msgs::image_encodings::RGB8) ||
            (image_encoding == sensor_msgs::image_encodings::RGB16))
            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB);

          if (
            (image_encoding == sensor_msgs::image_encodings::RGBA8) ||
            (image_encoding == sensor_msgs::image_encodings::RGBA16))
            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA);

          if (
            (image_encoding == sensor_msgs::image_encodings::BGRA8) ||
            (image_encoding == sensor_msgs::image_encodings::BGRA16))
            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA);
        } else {
          // if necessary convert colors from rgb to bgr
          if (
            (image_encoding == sensor_msgs::image_encodings::BGR8) ||
            (image_encoding == sensor_msgs::image_encodings::BGR16))
            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR);

          if (
            (image_encoding == sensor_msgs::image_encodings::BGRA8) ||
            (image_encoding == sensor_msgs::image_encodings::BGRA16))
            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA);

          if (
            (image_encoding == sensor_msgs::image_encodings::RGBA8) ||
            (image_encoding == sensor_msgs::image_encodings::RGBA16))
            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA);
        }
      }
    }
  } catch (cv::Exception & e) {
    ROS_ERROR("%s", e.what());
  }

  size_t rows = cv_ptr->image.rows;
  size_t cols = cv_ptr->image.cols;

  if ((rows > 0) && (cols > 0))
    // Publish message to user callback
    raw_image_pub_.publish(cv_ptr->toImageMsg());
}
}  // namespace image_preprocessor

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(image_preprocessor::ImageTransportDecompressorNodelet, nodelet::Nodelet)
